
#include "Self_Define_Functions.h"

using namespace std;


bool Compare_NewType( NewType data1, NewType data2 )
{
    return data1.data > data2.data;
}


double Fitness ( double *particle, int &Fes, CEC2013 *pFunc )
{
    Fes++;
    return pFunc->evaluate( particle );
}


void Popupation_Fitness( double **population, int population_size, int &Fes, double *result, CEC2013 *pFunc )
{
    int i;
    for ( i = 0; i < population_size; ++i )
    {
        result[i] = pFunc->evaluate( population[i] );

    }

    Fes += population_size;
}



double Distance( double *vector1, double *vector2, int dim )
{
    double sum = 0;
    for( int i = 0; i < dim; ++i )
    {
        sum += pow( vector1[i] - vector2[i], 2);
    }

    return sqrt( sum );
}

double Distance( double *vector1, vector<double> vector2, int dim )
{
    assert( vector2.size() == dim );

    double sum = 0;
    for( int i = 0; i < dim; ++i )
    {
        sum += pow( vector1[i] - vector2[i], 2);
    }

    return sqrt( sum );
}




void Get_Seeds( double **population, double *population_results, int population_size, int dim, vector<double> &seed_fitness, double radius )
{
    int i,j;
    bool found;
    double dist;

    vector< vector<double> > seeds;


    //sort population
    NewType *temp = new NewType [population_size];
    for( i = 0; i < population_size; ++i )
    {
        temp[i].data = population_results[i];
        temp[i].id = i;
    }


    sort( temp, temp+population_size, Compare_NewType );

    seed_fitness.clear();

    for( i = 0; i < population_size; ++i )
    {
        found = false;

        for( j = 0; j < seeds.size(); ++j )
        {
            dist = Distance( population[temp[i].id], seeds[j], dim );
            if( dist <= radius )
            {
                found = true;
                break;
            }
        }

        if( !found )
        {
            vector<double> temp_seed( population[temp[i].id], population[temp[i].id] + dim );
            seeds.push_back(temp_seed);
            seed_fitness.push_back( population_results[temp[i].id] );
        }

    }


    delete []temp;
    seeds.clear();
}





int How_Many_Global_Optima( vector<double> seed_fitness, double epsilon, CEC2013 *pFunc )
{

    int  counter = 0;;
	for ( unsigned int  i = 0; i < seed_fitness.size(); ++i )
	{
		/* |F_seed - F_goptimum| <= accuracy */
		if ( fabs(seed_fitness[i] - pFunc->get_fitness_goptima()) <= epsilon)
		{
			++counter;
		}
		/* save time */
		if (counter == pFunc->get_no_goptima())
            break;
	}
	return counter;

}


int Compute_Global_Optima_Found( double **population, double *population_results, int population_size, int dim, double epsilon, CEC2013 *pFunc )
{
    vector<double> seed_fitness;

    Get_Seeds( population, population_results,  population_size, dim, seed_fitness, pFunc->get_rho() );

    int counter = How_Many_Global_Optima( seed_fitness,  epsilon, pFunc );

    seed_fitness.clear();

    return counter;

}

int Close_Particle( double *child, double **population, int population_size, int dim )
{
    double min_distance = 1e100;

    int i;

    int particle_index = -1;

    double temp_distance;

    for( i = 0; i < population_size; ++i )
    {
        temp_distance = Distance( child, population[i], dim );
        if( min_distance > temp_distance )
        {
            min_distance = temp_distance;
            particle_index = i;
        }
    }


    return particle_index;

}


int Close_Particle( double *dist,  int population_size )
{
    double min_distance = dist[0];

    int i;

    int particle_index = 0;


    for( i = 1; i < population_size; ++i )
    {
        if( min_distance > dist[i] )
        {
            min_distance = dist[i];
            particle_index = i;
        }
    }


    return particle_index;

}


void Dist_Individual_to_All( double *dist, double *individual, double **population, int population_size, int dim )
{
    int i;
    for( i = 0; i < population_size; ++i )
    {
        dist[i] = Distance( individual, population[i], dim );
    }

}

void Dist_All_to_All( double **population, double **dist, int population_size, int dim )
{
    int i,j;
    for( i = 0; i < population_size; ++i )
    {
        dist[i][i] = 0;
    }

    for( i = 0; i < population_size; ++i )
    {
        for( j = i+1; j < population_size; ++j )
        {
            dist[i][j] = dist[j][i] = Distance( population[i], population[j], dim );
        }
    }
}


void Update_Dist( double **dist, double *temp_dist, int closet_individual, int population_size )
{
    int i;

    memcpy( dist[closet_individual], temp_dist, sizeof(double)*population_size );

    for( i = 0; i < population_size; ++i )
        dist[i][closet_individual] = temp_dist[i];
}

void Sort_Population_Dist( double *dist, int *sorted_index, int population_size )
{
    int i;

    NewType *temp = new NewType [population_size];

    for( i = 0; i < population_size; ++i )
    {
        temp[i].data = dist[i];
        temp[i].id = i;
    }


    sort( temp, temp+population_size, Compare_NewType );

    for( i = 0; i < population_size; ++i )
        sorted_index[i] = temp[population_size-1-i].id;

    delete []temp;
}

void Sort_Population_Fitness( double *population_results, int *sorted_index, int population_size )
{
    int i;

    NewType *temp = new NewType [population_size];

    for( i = 0; i < population_size; ++i )
    {
        temp[i].data = population_results[i];
        temp[i].id = i;
    }


    sort( temp, temp+population_size, Compare_NewType );

    for( i = 0; i < population_size; ++i )
        sorted_index[i] = temp[i].id;

    delete []temp;
}


void Get_Nearest_M_Individuals( double *dist, int *Nearest_M, int population_size, int n_nearest )
{
    int i;

    NewType *temp = new NewType [population_size];
    for( i = 0; i < population_size; ++i )
    {
        temp[i].data = dist[i];
        temp[i].id = i;
    }


    sort( temp, temp+population_size, Compare_NewType );

    for( i = 0; i < n_nearest; ++i )
    {
        Nearest_M[i] = temp[population_size-i-1].id;
    }

    delete []temp;

}

void Get_Nearest_M_Individuals( double *dist, bool *flag, int *Nearest_M, int population_size, int n_nearest )
{
    int i,j;

    NewType *temp = new NewType [population_size];
    for( i = 0; i < population_size; ++i )
    {
        temp[i].data = dist[i];
        temp[i].id = i;
    }


    sort( temp, temp+population_size, Compare_NewType );

    j = population_size-1;
    for( i = 0; i < n_nearest; ++i )
    {
        while( j >=0 )
        {
            if( !flag[temp[j].id] )
            {
                Nearest_M[i] = temp[j].id;
                flag[temp[j].id] = true;
                --j;
                break;
            }
            else
            {
                --j;
            }

        }

    }

    delete []temp;

}

void Species( double *reference_point, double **population, int **species, double **dist,
              int species_num, int N_nearest, int last_species_size, int population_size, int dim )
{
    int i,j;
    int start_search = 0;
    int final_size;


    bool *flag = new bool[population_size];
    int *sorted_index = new int [population_size];

    for( i = 0; i < population_size; ++i )
        flag[i] = false;

    double *ref_dist = new double[population_size];

    Dist_Individual_to_All( ref_dist, reference_point, population, population_size, dim );

    Sort_Population_Dist( ref_dist, sorted_index, population_size );

    start_search = 0;

    for( i = 0; i < species_num-1; ++i )
    {
        for( j = start_search; j < population_size; ++j )
        {
            if( !flag[sorted_index[start_search]] )
            {
                species[i][0] = sorted_index[start_search];//find seed
                break;
            }
        }

        Get_Nearest_M_Individuals( dist[species[i][0]], flag, species[i], population_size, N_nearest );

        //update start_search

        while( flag[sorted_index[start_search]] )
            ++start_search;
    }

    if( last_species_size == 0 )
    {
        final_size = N_nearest;
    }
    else
    {
        final_size = last_species_size;
    }

    for( j = 0; j < final_size; ++j )
    {
        while( flag[sorted_index[start_search]] )
            ++start_search;

        species[i][j] = sorted_index[start_search];
        flag[sorted_index[start_search]] = true;
        ++start_search;

    }


    delete []ref_dist;
    delete []sorted_index;
    delete []flag;
}

void Get_Seed_Location( int *seed_location, double *population_results, int **species, int species_num, int speices_size, int last_species_size )
{
    int i,j;
    double best;
    int final_size;

    for( i = 0; i < species_num-1; ++i )
    {
        best = population_results[species[i][0]];
        seed_location[i] = species[i][0];

        for( j = 1; j < speices_size; ++j )
        {
            if( best < population_results[species[i][j]] )
            {
                best = population_results[species[i][j]];
                seed_location[i] = species[i][j];
            }
        }
    }

    if( last_species_size == 0 )
    {
        final_size = speices_size;
    }
    else
    {
        final_size = last_species_size;
    }

    best = population_results[species[i][0]];
    seed_location[i] = species[i][0];

    for( j = 1; j < final_size; ++j )
    {
        if( best < population_results[species[i][j]] )
        {
            best = population_results[species[i][j]];
            seed_location[i] = species[i][j];
        }
    }


}


void Mean_Std( double *mean_value, double *std_value, int *species, double **population, int species_size, int dim )
{
    int i, j;

    if( species_size ==1 )
    {
        for( i = 0; i < dim; ++i )
        {
            std_value[i] = 1e-4;
        }
    }
    else
    {
        for( i = 0; i < dim; ++i )
        {
            mean_value[i] = 0;
            for( j = 0; j < species_size; ++j )
            {
                mean_value[i] += population[species[j]][i];
            }

            mean_value[i] /= species_size;
        }

        for( i = 0; i < dim; ++i )
        {
            std_value[i] = 0;
            for( j = 0; j < species_size; ++j )
            {
                std_value[i] += ( population[species[j]][i] - mean_value[i] ) * ( population[species[j]][i] - mean_value[i] );
            }

            std_value[i] = sqrt( std_value[i] / ( species_size -1 ) );

            if( std_value[i]== 0 )
                std_value[i] = 1e-4;
        }
    }
}


void Mean( double *mean_value, int *species, double **population, int species_size, int dim )
{
    int i, j;
    for( i = 0; i < dim; ++i )
    {
        mean_value[i] = 0;
        for( j = 0; j < species_size; ++j )
        {
            mean_value[i] += population[species[j]][i];
        }

        mean_value[i] /= species_size;
    }
}



void Species_Evolve_Gaussian( int *species, double **population, double **child, double *LBound, double *UBound,
                     int dim, int species_size, double std_factor )
{
    int i,j;

    boost::mt19937 generator(time(0)*rand());


    double *mean_value = new double[dim];

    double *std_value = new double[dim];

    Mean_Std( mean_value, std_value, species, population, species_size, dim );

    for( i = 0; i < dim; ++i )
    {
        boost::normal_distribution<> norm_generator( mean_value[i], std_value[i]*std_factor ); // this needs to be tuned
        boost::variate_generator< boost::mt19937&, boost::normal_distribution<> > norm_generation( generator, norm_generator );


        for( j = 0; j < species_size; ++j )
        {
            child[species[j]][i] = norm_generation();
            if( child[species[j]][i] < LBound[i] )
                child[species[j]][i] = LBound[i];

            if( child[species[j]][i] > UBound[i] )
                child[species[j]][i] = UBound[i];
        }

    }

    delete []mean_value;

    delete []std_value;


}

void Species_Evolve_Cauchy( int *species, double **population, double **child, double *LBound, double *UBound,
                     int dim, int species_size, double std_factor )
{
    int i,j;

    boost::mt19937 generator(time(0)*rand());


    double *mean_value = new double[dim];

    double *std_value = new double[dim];

    Mean_Std( mean_value, std_value, species, population, species_size, dim );

    for( i = 0; i < dim; ++i )
    {
        boost::cauchy_distribution<> cau_generator( mean_value[i], std_value[i]*std_factor ); // this needs to be cauchy_distribution
        boost::variate_generator< boost::mt19937&, boost::cauchy_distribution<> > cau_generation( generator, cau_generator );


        for( j = 0; j < species_size; ++j )
        {
            child[species[j]][i] = cau_generation();
            if( child[species[j]][i] < LBound[i] )
                child[species[j]][i] = LBound[i];

            if( child[species[j]][i] > UBound[i] )
                child[species[j]][i] = UBound[i];
        }

    }

    delete []mean_value;

    delete []std_value;


}



void Evolve_Gaussian( int **species, double **population, double **child, double *LBound, double *UBound,
             int dim, int species_num, int n_nearest, int last_species_size, double std_factor )
{
    int i;
    for( i = 0; i < species_num-1; ++i )
    {
         Species_Evolve_Gaussian( species[i], population, child, LBound, UBound, dim, n_nearest, std_factor );
    }

    if( last_species_size == 0 )
        Species_Evolve_Gaussian( species[i], population, child, LBound, UBound, dim, n_nearest, std_factor );
    else
        Species_Evolve_Gaussian( species[i], population, child, LBound, UBound, dim, last_species_size, std_factor );


}

void Evolve_Cauchy( int **species, double **population, double **child, double *LBound, double *UBound,
             int dim, int species_num, int n_nearest, int last_species_size, double std_factor )
{
    int i;
    for( i = 0; i < species_num-1; ++i )
    {
         Species_Evolve_Cauchy( species[i], population, child, LBound, UBound, dim, n_nearest, std_factor );
    }

    if( last_species_size == 0 )
        Species_Evolve_Cauchy( species[i], population, child, LBound, UBound, dim, n_nearest, std_factor );
    else
        Species_Evolve_Cauchy( species[i], population, child, LBound, UBound, dim, last_species_size, std_factor );


}

void Evolve_Cauchy_Gaussian( int **species, double **population, double **child, double *LBound, double *UBound,
             int dim, int species_num, int n_nearest, int last_species_size, double std_factor, double cauchy_gaussian_prob )
{
    int i;
    boost::mt19937 generator(time(0)*rand());
    boost::uniform_real<> uniform_real_generate_r( 0, 1 );
    boost::variate_generator< boost::mt19937&, boost::uniform_real<> > random_real_num_r( generator, uniform_real_generate_r );
    double temp_prob;

    int final_size;

    for( i = 0; i < species_num-1; ++i )
    {
        temp_prob = random_real_num_r();

        if( temp_prob < cauchy_gaussian_prob )
            Species_Evolve_Cauchy( species[i], population, child, LBound, UBound, dim, n_nearest, std_factor );
        else
            Species_Evolve_Gaussian( species[i], population, child, LBound, UBound, dim, n_nearest, std_factor );
    }

    if( last_species_size == 0 )
        final_size = n_nearest;
    else
        final_size = last_species_size;

    temp_prob = random_real_num_r();

    if( temp_prob < cauchy_gaussian_prob )
        Species_Evolve_Cauchy( species[i], population, child, LBound, UBound, dim, final_size, std_factor );
    else
        Species_Evolve_Gaussian( species[i], population, child, LBound, UBound, dim, final_size, std_factor );

}




void Selection( double **population, double **child, double **dist, double *population_results, double *child_results,
                int population_size, int dim )
{
    int i;

    int closest_index;

    double *temp_dist = new double[population_size];

    for( i =0 ; i < population_size; ++i )
    {
        Dist_Individual_to_All( temp_dist, child[i], population, population_size, dim );

        closest_index = Close_Particle( temp_dist, population_size );



        if( child_results[i] > population_results[closest_index] )
        {
            memcpy( population[closest_index], child[i], sizeof(double)*dim );
            population_results[closest_index] = child_results[i];
            temp_dist[closest_index] = 0;
            Update_Dist( dist, temp_dist, closest_index, population_size );

        }
    }

    delete []temp_dist;


}

void Seed_Local_Prob( double *seed_local_prob, int *seed_location, double *population_results, int species_num )
{
    int i;

    double min_value;
    double max_value;


    double normalize_factor;

    min_value = max_value = population_results[seed_location[0]];

    for( i = 1; i < species_num; ++i )
    {
        if( min_value > population_results[seed_location[i]] )
            min_value = population_results[seed_location[i]];
        if( max_value < population_results[seed_location[i]] )
            max_value = population_results[seed_location[i]];
    }

    if( min_value <= 0 )
    {
        normalize_factor = max_value + fabs(min_value)+1;
        for( i = 0; i < species_num; ++i )
        {
            seed_local_prob[i] = ( population_results[seed_location[i]] + fabs(min_value) +1 ) / normalize_factor;
        }
    }
    else
    {
        for( i = 0; i < species_num; ++i )
        {
            seed_local_prob[i] =  population_results[seed_location[i]] / max_value;
        }
    }


}


void Generate_Reference_Point( double *reference_point, double *LBound, double *UBound, int dim )
{
    int j;
    for( j =0; j < dim; ++j )
    {
        boost::mt19937 generator(time(0)*rand());
        boost::uniform_real<> uniform_real_generate_x( LBound[j], UBound[j] );
        boost::variate_generator< boost::mt19937&, boost::uniform_real<> > random_real_num_x( generator, uniform_real_generate_x );

        reference_point[j] = random_real_num_x();
    }
}


void Sort_Species( int *species, double *population_results, int *sorted_index, int species_size )
{
    int i;

    NewType *temp = new NewType [species_size];

    for( i = 0; i < species_size; ++i )
    {
        temp[i].data = population_results[species[i]];
        temp[i].id = i;
    }


    sort( temp, temp+species_size, Compare_NewType );

    for( i = 0; i < species_size; ++i )
        sorted_index[i] = temp[i].id;

    delete []temp;

}



void Local_Species_Evolve(  double **population, double *population_results, double *LBound, double *UBound, int seed_location,
                            int dim,  double local_std_value , int &Fes, CEC2013 *pFun, int sample_num )
{
    int j,k;
    boost::mt19937 generator(time(0)*rand());

    double *temp_particle = new double[dim];

    double temp_fitness ;


        for( k = 0; k < sample_num; ++k )
        {
            for( j = 0; j < dim; ++j )
            {

                boost::normal_distribution<> norm_generator( population[seed_location][j], local_std_value ); // this needs to be tuned
                boost::variate_generator< boost::mt19937&, boost::normal_distribution<> > norm_generation( generator, norm_generator );


                temp_particle[j] = norm_generation();
                if( temp_particle[j] < LBound[j] )
                    temp_particle[j] = LBound[j];

                if( temp_particle[j] > UBound[j] )
                    temp_particle[j] = UBound[j];
            }

            temp_fitness = pFun->evaluate(temp_particle);
            Fes++;
            if( temp_fitness > population_results[seed_location] )
            {
                population_results[seed_location] = temp_fitness;
                memcpy( population[seed_location], temp_particle, sizeof(double)*dim );
            }

        }



    delete []temp_particle;

}


void Local_Evolve(  double **population, double *population_results, double *LBound, double *UBound, double *seed_local_prob, int *seed_location,
                    int dim, int species_num,  double local_std_value, int &Fes, CEC2013 *pFun, int sample_num )
{
    int i;

    double temp_prob;

    boost::mt19937 generator(time(0)*rand());
    boost::uniform_real<> uniform_real_generate_r( 0, 1 );
    boost::variate_generator< boost::mt19937&, boost::uniform_real<> > random_real_num_r( generator, uniform_real_generate_r );



    for( i = 0; i < species_num; ++i )
    {
        temp_prob = random_real_num_r();
        if( temp_prob <= seed_local_prob[i] )
            Local_Species_Evolve(  population, population_results, LBound, UBound, seed_location[i], dim, local_std_value, Fes, pFun, sample_num );
    }


}



























